'''机器人控制模块'''

from cmath import sqrt
from collections import namedtuple
from time import sleep
import jkrc
import numpy as np
import copy
import time
from configs import logger
robot_IP = "192.168.3.4"
PI=3.1415926
#坐标系
COORD_BASE = 0
COORD_JOINT = 1
COORD_TOOL = 2

 #运动模式
ABS = 0
INCR= 1
SPEED = 100 #mm/s

IO_CABINET = 0 #控制柜面板 IO
'''jog 运动'''
'''1.关节空间 jog'''

EORROR_CODE = [] #错误码


# import logging
# logging.basicConfig(level = logging.INFO,format = '%(asctime)s - %(name)s - %(levelname)s - %(message)s')
# logger = logging.getLogger(__name__)

ErrorCode = namedtuple('ErrorCode',['type','value','massage'])
errorcode_list = [ErrorCode._make(['ERR_SUCC',0,'成功']),
 ErrorCode._make(['ERR_INVALID_HANDLER',-1,'无效的句柄']),
 ErrorCode._make(['ERR_INVALID_PARAMETER',-2,'无效的参数']),
 ErrorCode._make(['ERR_COMMUNICATION_ERR',-3,'通信错误']),
 ErrorCode._make(['ERR_KINE_INVERSE_ERR',-4,'逆解失败']),
 ErrorCode._make(['ERR_EMERGENCY_PRESSED',-5,'急停键没有松开']),
 ErrorCode._make(['ERR_NOT_POWERED',-6,'机器人未上电']),
 ErrorCode._make(['ERR_NOT_ENABLED',-7,'机器人未使能']),
 ErrorCode._make(['ERR_DISABLE_SERVOMODE',-8,'未进入SERVOMODE模式']),
 ErrorCode._make(['ERR_NOT_OFF_ENABLE',-9,'关闭电源前未先下使能']),
 ErrorCode._make(['ERR_PROGRAM_IS_RUNNING',-10,'程序正在运行']),
 ErrorCode._make(['ERR_CANNOT_OPEN_FILE',-11,'打开文件失败']),
 ErrorCode._make(['ERR_MOTION_ABNORMAL',-12,'运动过程中发生异常']),
 ErrorCode._make(['RR_FTP_PREFROM',-14,'ftp异常']),
]
def showcode(code,detail=""):
    for errorcode in errorcode_list:
        if errorcode.value == code[0]:
            logger.info(errorcode.massage+detail)
            return code
    else:
        logger.info(detail)
        return code
    assert "Unknown errorcode"
    

class HandRobot:
    def __init__(self,ip=robot_IP):
        self.ready = False
        self.ip = ip
        self.robot = jkrc.RC(self.ip)
        self.set_robot_do_flag = False
        self.set_robot_do_coord_flag=False
        # self.robot_get=0
        # self.robot_done=0
        try:
            self.login()
        except Exception as e:
            print(e)
            self.login()
        self.ready = True
        _,self.ret = self.robot.get_robot_status()

        if self.ret in EORROR_CODE:
            raise self.ret
        else:
            self.init_robot_status()

    def init_robot_status(self):
        self.errcode            = self.ret[0]           #机器人运行出错时错误编号，0 为运行正常，其它为异常
        self.inpos              = self.ret[1]           #机器人运动是否到位标志，0 为没有到位，1 为运动到位
        self.powered_on         = self.ret[2]           #机器人是否上电标志，0 为没有上电，1 为上电
        self.enabled            = self.ret[3]           #机器人是否使能标志，0 为没有使能，1 为使能
        self.protective_stop    = self.ret[5]           #机器人是否检测到碰撞，0 为没有检测到碰撞，1 则相反
        self.drag_status        = self.ret[6]           #机器人是否处于拖拽状态，0 为没有处于拖拽状态，1 则相反
        self.on_soft_limit      = self.ret[7]           #机器人是否处于限位，0 为没有触发限位保护，1 为触发限位保护
        #情况信号
        self.set_AO(3,0,"清空信号")
        self.set_AO(4,0,"清空信号")
        self.set_AO(5,0,"清空信号")

    # 机器人登录、上电、上使能
    def login(self):
        code_login = self.robot.login() #登录
        showcode(code_login,detail="_"+"login")
        code_power_on = self.robot.power_on() #上电
        showcode(code_power_on,detail="_"+"power_on")
        # code_enable_robot = self.robot.enable_robot() #上使能
        # showcode(code_enable_robot,detail="_"+"enable_robot")

    # 机器人下使能、下电、注销
    def logout(self):
        code_disable_robot = self.robot.disable_robot() #下使能
        showcode(code_disable_robot)
        code_power_of = self.robot.power_off()#下电
        showcode(code_power_of)
        code_logout = self.robot.logout() #注销
        showcode(code_logout)

    def get_collision_level(self):
        ret = self.robot.get_collision_level()#获取当前碰撞等级
        if ret[0]==0:
            return ret[1]
        else:
            return None
        
    def recover_collision(self):
        ret = self.robot.collision_recover()
        if ret[0]==0:
            return True
        else:
            return False
        
    def check_collision(self):
        _,collision_status = self.robot.is_in_collision() #是否处于碰撞模式
        # self.robot.set_collision_level(3)#设置当前碰撞等级
        if _==0 :
            if collision_status ==1:
                return True
            else:
                return False
        else:
            return None
            
            # sleep(1)

    def load_program(self,program_name):
        ret = self.robot.program_load(program_name)#加载通过 APP 编写的脚本 program_test 需要自己编写
        self.robot.set_errorcode_file_path("JAKA_ERROR_CODE.csv") #
        self.robot.get_last_error () 
        self.robot.get_loaded_program() 
        print("the loaded program is:",ret)
        self.check_collision()
        self.robot.program_run()
        while 1:
            code,state = self.robot.get_program_state()
            if code==0:
                logger.info("机器状态:{}".format(state))
                order = input("1.pause 2.resume 3.abort 4.start 5.exit\n")
                if order=="1":
                    self.robot.program_pause()
                if order=="2":
                    self.robot.program_resume()
                if order=="3":
                    self.robot.program_abort()
                if order=="4":
                    self.robot.program_run()
                if order=="5":
                    self.robot.program_abort()
                    break


    def get_AO(self,AO_index,msg=""):
        # 
        ret = self.robot.get_analog_output(iotype = IO_CABINET,index = AO_index-1)
        if ret[0]==0:
            # print("获取成功:{}AO{}-位置{}".format(msg,AO_index,ret[1]))
            return ret[1]
        else:
            # print("获取失败:{}AO{}".format(msg,AO_index))
            return None

    def set_AO(self,AO_index,value,msg=""):
        ret = self.robot.set_analog_output(iotype = IO_CABINET,index = AO_index-1,value = value)
        if ret[0]==0:
            # print("设置成功:{}AO{}-动作{}".format(msg,AO_index,value))
            return True
        else:
            # print("设置失败:{}AO{}-动作{}".format(msg,AO_index,value))
            return False

    def set_DO(self,DO_index,value,msg=""):
        ret = self.robot.set_digital_output(IO_CABINET, DO_index-1, value)#设置 DO2 的引脚输出值为 1
        if ret[0]==0:
            # print("设置成功:{}DO{}-动作{}".format(msg,DO_index,value))
            return True
        else:
            # print("设置失败:{}DO{}-动作{}".format(msg,DO_index,value))
            return False

    def get_DO(self,DO_index,msg=""):
        ret = self.robot.get_digital_output(IO_CABINET, DO_index-1)
        if ret[0]==0:
            # print("获取成功:{}DO{}-位置{}".format(msg,DO_index,ret[1]))
            return ret[1]
        else:
            # print("获取失败:{}AO{}".format(msg,DO_index))
            return None
        
    def robot_do(self,cook_id:int,msg=""):#
        print("执行{}".format(msg))
        if self.set_AO(3,cook_id,msg="设置目标动作"):
            while True:
                time.sleep(0.1)
                robot_get = self.get_AO(4,msg="获取机器人接收到的位置")
                robot_done = self.get_AO(5,msg="获取机器人所到达的位置")
                if robot_get is not None:
                    if int(robot_get)==cook_id:
                        self.set_AO(3,0,msg="清空目标动作")
                    if robot_done is not None:
                        if int(robot_done) == cook_id:
                            return True
        return False
    
    # def robot_do(self,cook_id:int,msg=""):#
    #     if not self.set_robot_do_flag:
    #         print("执行{}".format(msg))
    #         self.set_robot_do_flag = self.set_AO(3,cook_id,msg="设置目标动作")
    #     robot_get = self.get_AO(4,msg="获取机器人接收到的位置")
    #     robot_done = self.get_AO(5,msg="获取机器人所到达的位置")

    #     if robot_get is not None:#机器人接收到的位置
    #         if int(robot_get)==cook_id:
    #             self.set_AO(3,0,msg="清空目标动作")
    #     if robot_done is not None:#机器人到达的位置
    #         if int(robot_done) == cook_id:
    #             self.set_robot_do_flag=False
    #             print(".........")
    #             return True
    #     return False

    def set_robot_do(self,cook_id:int,msg=""):
        if not self.set_robot_do_flag:
            
            self.set_robot_do_flag = self.set_AO(3,cook_id,msg="设置目标动作")
            if self.set_robot_do_flag:
                logger.info(msg+":"+"完成")

    def get_robot_do_done(self,cook_id:int,msg=""):
        if self.set_robot_do_flag and cook_id:
            robot_get = self.get_AO(4,msg="获取机器人接收到的位置")
            robot_done = self.get_AO(5,msg="获取机器人所到达的位置")
            # print(""robot_get,robot_done)
            if robot_get is not None:#机器人接收到的位置
                logger.info(msg+":"+"进行中")
                if int(robot_get)==cook_id:
                    self.set_AO(3,0,msg="清空目标动作")
            if robot_done is not None:#机器人到达的位置
                if int(robot_done) == cook_id:
                    logger.info(msg+":"+"完成")
                    self.set_robot_do_flag=False
                    return True
        return False
    
    def set_robot_do_coord(self,plate_coord,msg=""):#
        if not self.set_robot_do_coord_flag:
            if self.set_AO(19,plate_coord[0],msg="放餐位置X") and self.set_AO(20,plate_coord[1],msg="放餐位置Y"):
                self.set_robot_do_coord_flag=True
                logger.info(msg+":"+"完成")
    
    # def get_robot_do_coord_done(self,plate_coord,msg=""):
    #     if self.set_robot_do_coord_flag:
    #         self.set_robot_do(7,msg="倒餐位置")
    #         self.get_robot_do_done(7)

    # def robot_do_coord(self,plate_coord,msg=""):#
    #     if not self.set_robot_do_coord_flag:
    #         print("执行{}-坐标{}".format(msg,plate_coord))
    #         if self.set_AO(19,plate_coord[0],msg="放餐位置X") and self.set_AO(20,plate_coord[1],msg="放餐位置Y"):
    #             self.set_robot_do_coord_flag=True
    #     if self.set_robot_do_coord_flag:
    #         if self.robot_do(7,msg="到达倒餐位置"):
    #             self.set_robot_do_coord_flag=False
    #             return True
    #         else:
    #             return False
    #     return False
    
    # def robot_do_coord(self,plate_coord,msg=""):#
        
    #     if not self.set_robot_do_coord_flag:
    #         print("执行{}-坐标{}".format(msg,plate_coord))
    #         if self.set_AO(19,plate_coord[0],msg="放餐位置X") and self.set_AO(20,plate_coord[1],msg="放餐位置Y"):
    #             self.set_robot_do_coord_flag=True
    #     if self.set_robot_do_coord_flag:
    #         if self.robot_do(7,msg="到达倒餐位置"):
    #             self.set_robot_do_coord_flag=False
    #             return True
    #         else:
    #             return False
    #     return False

    def daocai(self,daocai_ready):
        if daocai_ready:
            self.set_robot_do(8,"设置倒菜动作")
            if self.get_robot_do_done(8,"执行倒餐动作"):
                return True
            else:
                return False
        else:
            return False

    # def daocai(self,daocai_ready):
    #     while True:
    #         time.sleep(0.1)
    #         if daocai_ready:
    #             if self.robot_do(8,"倒餐动作"):
    #                 return True
    #             else:
    #                 return False
    #         else:
    #             return False

    def robot_do_cook(self,cook_id,plate_coord,msg=""):#一轮打餐
        print("取餐{}-出餐{}".format(cook_id,plate_coord))
        if self.robot_do(cook_id,"取餐路径") and self.robot_do_coord(plate_coord,"出餐路径"):
            return True
        else:
            return False

    # def get_AO_forever(self):
    #     self.robot_get = self.get_AO(4,msg="获取机器人接收到的位置")
    #     self.robot_done = self.get_AO(5,msg="获取机器人所到达的位置")
if __name__ == "__main__":

    handrobot = HandRobot(robot_IP)
    # while True:
    handrobot.robot_do(9,msg = "回安全位置")
    # handrobot.robot_do_coord((-555,-145),msg="放到餐盘1号位")
    # handrobot.daocai(True)

    # handrobot.robot_do(2,msg = "2号位取餐")
    # handrobot.robot_do_coord((-555,-35),msg="放到餐盘2号位")
    # handrobot.daocai(True)

    # handrobot.robot_do(3,msg = "3号位取餐")
    # handrobot.robot_do_coord((-555,65),msg="放到餐盘3号位")
    # handrobot.daocai(True)

    # handrobot.robot_do(4,msg = "4号位取餐")
    # handrobot.robot_do_coord((-430,25),msg="放到餐盘4号位")
    # handrobot.daocai(True)

    # handrobot.robot_do(5,msg = "5号位取餐")
    # handrobot.robot_do_coord((-555,-145),msg="放到餐盘1号位")
    # handrobot.daocai(True)

    # handrobot.robot_do(6,msg = "6号位取餐")
    # handrobot.robot_do_coord((-555,-35),msg="放到餐盘2号位")
    # handrobot.daocai(True)

    # print(handrobot.check_collision())
    # handrobot.logout()
    # print(handrobot.robot.disable_robot()) #下使能())
    # print(handrobot.recover_collision())
    # while True:
    #     handrobot.robot_do_cook(True,1,(150,450))
    #     handrobot.robot_do_cook(True,1,(0,350))
    #     handrobot.robot_do_cook(True,1,(-150,450))
    #     handrobot.robot_do_cook(True,2,(150,450))
    #     handrobot.robot_do_cook(True,2,(0,350))
    #     handrobot.robot_do_cook(True,2,(-150,450))
    #写
    #DO1 允许倒餐
    #AO3 目标动作
    #AO19 倒餐X偏移量
    #AO20 倒餐Y偏移量

    #读
    #AO4 接收到的位置，与AO3相同需清空AO3
    #AO5 到达的位置，与AO3相同则到达完成
    # import jkrc
    # robot = jkrc.RC("192.168.3.4") #返回一个机器人对象
    # print(robot)
    # print(robot.login())#登录

    # print(robot.power_on()) #上电
    # print(robot.enable_robot())
    

    # print(robot.set_digital_output(0, 0, 1))#设置 DO1 的引脚输出值为1)
    # print(robot.get_digital_output(0, 0))
    
    # print(robot.disable_robot() )
    # print(robot.power_off())#下电
    # print(robot.logout()) #登出
   

